/*
* $Id: unet.c,v 1.98 2008/03/10 05:31:10 sunny Exp $
*/
/*******************************************************************************

       UBEC (Uniband Electronic Corp.)

       Project: U-NET01, Ubiquitous network platform

       File: unet.c

       Version: 0.3.0

       Usage: Unet api & unet sub function

       Platform: U-NET01 DK with Keil 8051 C compiler

       Reference:

               Silicon Laboratories: C8051F124, C8051F340

               UBEC: UZ2400, UZ2410

       Note :

               Copyright (C) 2007 Uniband Electronic Corporation, All rights reserved

********************************************************************************/

#if defined(__K0__)
#pragma section @@DATA @@IXDATA
//#pragma inline  // enable memcpy() and memset() with direct inline expansion instead of function call
#endif // #if __K0__

#include "unet.h"

#if 0 // DEBUG_MODE
	#undef  TRACE_TAG
	#define TRACE_TAG()
	#undef  TRACE_puts
	#define TRACE_puts              DBG_puts
	#undef  TRACE_printf
	#define TRACE_printf(P)         dbg_printf P
#endif

UNET_SYS UnetSys;
POLL poll;
#if !USE_BEACON_LIST
BEACON beacon;
#endif
//UINT8 LED_INDEX;
UINT8 LQIValue;
UINT8 RSSIValue;
UINT16 RecycleAddr;
UINT8 AddrRecycleFlg;

UINT16 *RecycleAddress;
UINT8 RecycleIndex;

//UINT16 RejoinParent;

UINT16 tx_failure_address;

TIMER *poll_tm;
TIMER *gettable_tm;
TIMER *reqtable_tm;
TIMER *chdalive_tm;

#if DEBUG_FUNC
TIMER *rssi_dbg_tm;
TIMER *child_number_dbg_tm;
uint8 dbg_child_number_flag;
uint8 dbg_child_number;
#endif


MSG_QUEUE CommQueue; // Mac command queue
//MSG_QUEUE DataQueue; // Mac data queue

//UINT8 RF_RX_BUFF[128+16]; // RF receive buffer
UINT8 RF_TX_BUFF[128]; // RF transmit buffer

/*******************************************************************************/



/*******************************************************************************

	Unet Function

********************************************************************************/

UINT8 UNET_INIT(UINT16 PanId, UINT8 Channel, UINT16 NewParent, UINT8 MaxChild, UINT8 MaxRouter, UINT8 MaxDepth)
{
	TRACE_printf(("\rInit UNET, PanId=%u (0x%04x), Channel=%u, NewParent=%04x", PanId, PanId, (UINT16)Channel, NewParent));

	if(PanId > 0x3fff) {
		TRACE_puts("PANID_INVALID, Enable AUTO set");
		UnetSys.JoinPID = 0xffff;
	}

	if(Channel > 26 || Channel < 11) {
		TRACE_puts("CHANNEL_INVALID, Enable AUTO set");
		UnetSys.Channel = 0xff;
	}

	User_LED_Off();

	UzInit(); //RF Initial

#ifdef TURBO
	UzEnabTurboMode();
#endif

	UzSetMacAddress(MAC_ADDRESS); //MAC Address

	unet_initial(); //UNET Initial

	UnetSys.JoinPID = PanId;
	UnetSys.Channel = Channel;
	UnetSys.RejoinParent = NewParent;
	UnetSys.MaxChild = MaxChild;
	UnetSys.MaxRouter = MaxRouter;
	UnetSys.MaxDepth = MaxDepth;

	RecycleAddress = malloc(UnetSys.MaxChild*2);
	memset(RecycleAddress, 0xff, UnetSys.MaxChild*2);

	return SUCCESS;
}

#ifdef I_Am_Coordinator
UINT8 UNET_ESTABLISH(void) {
#if USE_BEACON_LIST
	struct _beacon_list_ *TmPtr;
#endif // #if USE_BEACON_LIST

	DBG_printf(("\rEstablish UNET, PanId=%u (0x%04x), Channel=%u: ", UnetSys.JoinPID, UnetSys.JoinPID, (UINT16)(UnetSys.Channel)));

	UnetSys.Flag.PanIdConflict = 0;

	unet_active_scan();

#if USE_BEACON_LIST
	if((TmPtr = UnetSys.BcnTail) != NULL) { // Check received beacon
	    do {
		TmPtr = TmPtr->Next;
		if(TmPtr->Channel == UnetSys.Channel && TmPtr->PID == UnetSys.JoinPID /* && BcnPkt->SrcPID == UNET_COORD_ADDRESS */) {
			UnetSys.Flag.PanIdConflict = 1;
			break;
		}
	    } while(TmPtr != UnetSys.BcnTail);
	}
#endif // #if USE_BEACON_LIST

	if(UnetSys.Flag.PanIdConflict) {
	#ifdef CONSOLE
		puts("ESTABLISH UNET ERROR: PAN ID CONFLICT\r\n");
	#endif

		UnetSys.JoinPID = 0xffff;
		return PAN_ID_CONFLICT;
	}

	UnetSys.NwkAddr = UNET_COORD_ADDRESS; // Coordinator default address
	UnetSys.DevDep = 0;
	UnetSys.NextAddr = UnetSys.NwkAddr + 1;
	UnetSys.AddrInc = compute_Cskip(UnetSys.DevDep);
	UnetSys.BcnPayload.DevDep = UnetSys.DevDep;
	UnetSys.BcnPayload.EndevCap = 1;
	UnetSys.BcnPayload.ProtID = 0;
	UnetSys.BcnPayload.ProtVer = 0x01;
	UnetSys.BcnPayload.StackPro = 0;
	UnetSys.BcnPayload.RouterCap = 1;
	UnetSys.BcnPayload.Rvd0 = 0;
	UnetSys.Flag.JoinNetwork = 1;
	UnetSys.Flag.PanCoordinator = 1;

	UnetSys.Flag.PermitJoin = 1;
	UnetSys.Flag.NwkReady = 1;
	UnetSys.RejoinParent = 0xffff;

#if USE_BEACON_LIST
	clear_table((void *) &UnetSys.BcnTail);
#endif

//	UzSetPanId(UnetSys.JoinPID);
	UzSetChannel(UnetSys.Channel);
	UzSetNwkAddr(UnetSys.NwkAddr);
	UzSetCoordinator();

	UzRxFlush();

	Link_LED_On();
#ifdef CONSOLE
	puts("\rESTABLISH UNET OK, Channel=");
	print_int(UnetSys.Channel);

	puts(", PAN ID=0x");
	print_hex((UINT8)(UnetSys.JoinPID >> 8));
	print_hex((UINT8)(UnetSys.JoinPID & 0xff));
	putchar('\r');
#endif
	return SUCCESS;
}

#else
UINT8 UNET_JOIN(void)
{
#if USE_BEACON_LIST
	struct _beacon_list_ *BcnPtr;
	UINT8 rssi = 0;
#endif
	TIMER *tm;

	DBG_printf(("\rJoin UNET, PanId=%u (0x%04x), Channel=%u: ", UnetSys.JoinPID, UnetSys.JoinPID, (UINT16)(UnetSys.Channel)));

	unet_active_scan();

#if USE_BEACON_LIST
	if((BcnPtr = UnetSys.BcnTail) != NULL) {
	    do {
		BcnPtr = BcnPtr->Next;
		if(!((UnetSys.RejoinParent == 0xffff) || (BcnPtr->Addr == UnetSys.RejoinParent)))
			continue;
		if((BcnPtr->Channel == UnetSys.Channel) && (BcnPtr->PID == UnetSys.JoinPID) && (BcnPtr->SfSpec & 0x8000) && (BcnPtr->LinkQuality > rssi) && (BcnPtr->DevDep < UnetSys.MaxDepth)){
			rssi = BcnPtr->LinkQuality;
			UnetSys.JoinParent = BcnPtr->Addr;
			UnetSys.DevDep = BcnPtr->DevDep + 1;
			UnetSys.Flag.FindNetwork = 1;
		}
	    } while(BcnPtr != UnetSys.BcnTail);
	}
#endif

	while(UnetSys.Flag.FindNetwork) {
		UnetSys.Flag.FindNetwork = 0; //Clear flag

	#if USE_BEACON_LIST
		clear_table((void *) &UnetSys.BcnTail);
	#else
		UnetSys.Channel = beacon.Channel;
		UnetSys.JoinPID = beacon.PID;
		UnetSys.JoinParent = beacon.Addr;
		UnetSys.DevDep = beacon.DevDep + 1;

		memset((void *) &beacon, 0, sizeof(beacon));
		beacon.DevDep = 0xff;
		beacon.ChildCnt = 0xff;
	#endif

		UzSetChannel(UnetSys.Channel);
		UzSetPanId(UnetSys.JoinPID);

		// Send association request
		UnetSys.Flag.RecvAssoRsp = 0; // Clear flag
		UnetSys.Flag.AssoSuccess = 0; // Clear flag
		unet_tx_asso_req();

		tm = setup_timer(UNET_SCAN_WAIT_TIME, false, NULL, NULL);
		while(!is_timer_expired(tm)) {
			unet_proc();	//Process received packets as soon as possible
			if(UnetSys.Flag.RecvAssoRsp) {
				break;
			}
		}
		close_timer(tm);

	#if USE_DENIER_LIST
		if(!UnetSys.Flag.RecvAssoRsp || (UnetSys.NwkAddr == 0xffff)) {	// 2007/7/9 16:56, Sunny: the association request being denied.
			TRACE_printf(("UNET_JOIN: add denier 0x%04X", UnetSys.JoinParent));
			if(insert_node((void *)&UnetSys.DenierTail, sizeof(DENIER_LIST_T)) == NULL) {
				TRACE_puts("UNET_JOIN: insert_node() FAILURE");
				return FAILURE;
			}
			UnetSys.DenierTail->PID = UnetSys.JoinPID;
			UnetSys.DenierTail->Addr = UnetSys.JoinParent;
			UnetSys.DenierTail->Channel = UnetSys.Channel;
			UnetSys.DenierTail->MaxDeny = MAX_DENY_COUNT;

			if(UnetSys.Flag.FindNetwork)
				continue;	// try another candidate parent node

			NetworkReset();
			TRACE_puts("UNET_JOIN: Network reset");
			return FAILURE;
		}
	#endif // #if USE_DENIER_LIST

		if(UnetSys.Flag.AssoSuccess){ //Check associate result
			UnetSys.NextAddr = UnetSys.NwkAddr + 1;
			UnetSys.AddrInc = compute_Cskip(UnetSys.DevDep);
			UnetSys.BcnPayload.DevDep = UnetSys.DevDep;
			UnetSys.BcnPayload.EndevCap = 1;
			UnetSys.BcnPayload.ProtID = 0;
			UnetSys.BcnPayload.ProtVer = 0x01;
			UnetSys.BcnPayload.StackPro = 0;
			UnetSys.BcnPayload.RouterCap = 1;
			UnetSys.BcnPayload.Rvd0 = 0;
			UnetSys.Flag.JoinNetwork = 1;
			UnetSys.Flag.NwkReady = 1;
			UnetSys.RejoinParent = 0xffff;
			UnetSys.Flag.Router = 1;
			if(UnetSys.DevDep < UnetSys.MaxDepth)
				UnetSys.Flag.PermitJoin = 1;

			UzSetNwkAddr(UnetSys.NwkAddr);

		#if USE_DENIER_LIST
			clear_table((void *) &UnetSys.DenierTail);
		#endif

			UzRxFlush();

			Link_LED_On();
		#ifdef CONSOLE
			puts("\rJOIN UNET OK, Channel=");
			print_int(UnetSys.Channel);

			puts(", PAN ID=0x");
			print_hex((UINT8)(UnetSys.JoinPID >> 8));
			print_hex((UINT8)(UnetSys.JoinPID & 0xff));
			putchar('\r');
		#endif
			return SUCCESS;
		}

		UnetSys.RejoinParent = 0xffff;
	#ifdef CONSOLE
		puts("JOIN UNET FAILED: FAILURE\r\n");
	#endif
		NetworkReset();
		TRACE_puts("UNET_JOIN: Network reset");
		return FAILURE;
	}

	UnetSys.RejoinParent = 0xffff;
#ifdef CONSOLE
	puts("JOIN UNET FAILED: NETWORK NOT FOUND\r\n");
#endif
	NetworkReset();
	TRACE_puts("UNET_JOIN: Network reset");
	return NETWORK_NOT_FOUND;
}
#endif // #ifdef I_Am_Coordinator

UINT8 *RECV_FROM_UNET(UINT8 *Length)
{
	void *MsgPtr;
	struct _data_list_ *TmPtr;
	// UINT8 Length;

	if(UnetSys.DataTail){
		TRACE_puts("RECV_FROM_UNET");

		TmPtr = UnetSys.DataTail->Next;
		if(Length)
			*Length = TmPtr->Length;

	//	MSG_Copy(Buff, TmPtr->Msg, Length);
		MsgPtr = TmPtr->Msg;

	//	UnetSys.DataTail = TmPtr->Next;
	//	UnetSys.DataCnt -= 1;

	//	MSG_Free(TmPtr->Msg);
	//	MSG_Free(TmPtr);
		delete_node((void *)&UnetSys.DataTail, (void *)&TmPtr);

		return MsgPtr;
	}

	return NULL;
}

UINT8 CHECK_UNET_DATA(void)
{

	unet_proc();

	return(UnetSys.DataTail ? 1 : 0);
}

/*******************************************************************************

	Unet Sub Function

********************************************************************************/

void unet_initial(void)
{
	struct _data_list_ *Temp;
#if USE_DENIER_LIST
	DENIER_LIST_T *denier_list = UnetSys.DenierTail;
#endif
	TIMER *timer_list = UnetSys.TimerTail;

	TRACE_puts("unet_initial");

#if USE_BEACON_LIST
	clear_table((void *) &UnetSys.BcnTail);
#endif
	clear_table((void *) &UnetSys.ChildTail);
	clear_table((void *) &UnetSys.BroTail);
//	clear_table((void *) &UnetSys.TimerTail);

	TRACE_puts("cleanup Msg");
	while(UnetSys.DataTail) {
		Temp = UnetSys.DataTail->Next;
	//	UnetSys.DataTail->Next = Temp->Next;
		MSG_Free(Temp->Msg);
	//	MSG_Free(Temp);
		delete_node((void *)&UnetSys.DataTail, (void *)&Temp);
	}
//	EnabInt();

	// 2007/8/29 11:34, Sunny: all link lists MUST be deleted at this point!
//	MSG_Init(&CommQueue);
//	MSG_Init(&DataQueue);

	free(RecycleAddress);
	init_unet_vars();

	UnetSys.TimerTail = timer_list;
#if USE_DENIER_LIST
	UnetSys.DenierTail = denier_list;
#endif
}

void init_unet_vars(void)
{
	TRACE_puts("init_unet_vars");

//	memset(&UnetSys, 0, sizeof(UNET_SYS));
	memset(&CommQueue, 0, sizeof(MSG_QUEUE));
//	memset(&DataQueue, 0, sizeof(MSG_QUEUE));

	memset((void *) &UnetSys, 0, sizeof(UnetSys));

	memset((void *) &poll, 0, sizeof(poll));

	poll.ETA = UNET_POLL_INTERVAL;
#if !USE_BEACON_LIST
	memset((void *) &beacon, 0, sizeof(beacon));
	beacon.DevDep = 0xff;
	beacon.ChildCnt = 0xff;
#endif
//	RecycleAddress = malloc(UnetSys.MaxChild*2);
//	memset(RecycleAddress, 0xff, UnetSys.MaxChild*2);

	UnetSys.JoinParent = 0xffff;	// Join parent address
	UnetSys.OrigParent = 0xffff;
	UnetSys.NwkAddr = 0xffff;	// Network address (short address)
	UnetSys.DevDep = 0xff;		// Device depth

	RecycleIndex = 0;
	AddrRecycleFlg = 0;
	RecycleAddr = 0xffff;
	RecycleAddress = NULL;

#if DEBUG_FUNC
	dbg_child_number_flag = 0;
	dbg_child_number = 0;
#endif
}

UINT8 unet_check_child(UINT16 NwkAddr, struct _child_list_ **CurrPtr){
	struct _child_list_ *ChildPtr;

	TRACE_printf(("unet_check_child: %04X, ", NwkAddr));

	if((ChildPtr = UnetSys.ChildTail) != NULL) {
	    do {
		ChildPtr = ChildPtr->Next;
		TRACE_printf((" %04X:%d", ChildPtr->NetworkAddress, (UINT16)ChildPtr->Available));
		if(ChildPtr->NetworkAddress == NwkAddr){
			if(CurrPtr != NULL)
				*CurrPtr = ChildPtr;

			if(ChildPtr->Available) {
			//	TRACE_puts("unet_check_child: SUCCESS");
				return SUCCESS;
			}
			DBG_puts(" ==> unavailable");
			break;
		}
	    } while(ChildPtr != UnetSys.ChildTail);
	}

	TRACE_puts("unet_check_child: FAILURE");
	return FAILURE;
}

UINT16 unet_tree_routing(UINT16 DstAddr)
{
	UINT16 NextAddr, Cskip, A;

	TRACE_puts("unet_tree_routing");

	A = UnetSys.NwkAddr;

	Cskip = compute_Cskip(UnetSys.DevDep -1);  //Cskip(d-1)

	DBG_printf(("\rD=%d, A=%d, Cskip=%d, UnetSys.DevDep=%d", DstAddr, A, Cskip, UnetSys.DevDep));
	if((DstAddr > A) && (DstAddr < (A + Cskip))){ //if destination is my descendant? ( A<D<A+compute_Cskip(d-1)), 2.7.3.3
		Cskip = compute_Cskip(UnetSys.DevDep);
		NextAddr = (A + 1) + ((DstAddr - (A + 1)) / Cskip) * Cskip;

		if(NextAddr < UnetSys.NextAddr){ // if next hop addr alread allocate?
//			if(unet_check_child(NextAddr, NULL)) {
				// DstAddr = NextAddr;
				TRACE_puts("unet_tree_routing: FOUND_NEXT_HOP");
				return(NextAddr);	// FOUND_NEXT_HOP;
//			}
		}

		TRACE_puts("unet_tree_routing: NEXT_HOP_NOT_AVAILABLE");
		return((UINT16)(-1));	// NEXT_HOP_NOT_AVAILABLE;
	}

	if(UnetSys.Flag.PanCoordinator) {
		TRACE_puts("unet_tree_routing: TREE_ROUTING_FAILED");
		return((UINT16)(-1));	// TREE_ROUTING_FAILED;
	}

	// DstAddr = UnetSys.JoinParent; // Send to parent
	TRACE_printf(("unet_tree_routing: SEND_TO_PARENT: %04X", DstAddr));
	return(UnetSys.JoinParent);	// SEND_TO_PARENT;
}

/*******************************************************************************

	MAC PROCESS FUNCTION

********************************************************************************/

INT8 unet_queue_proc(UINT8 frequency, MSG_QUEUE *QueuePtr){
	INT8 ReadPtr;
	void **MsgPtr;
	UINT8 MsgLength;

	while(frequency >0 && QueuePtr->QueueDataCount){
		if((ReadPtr = MSG_Fetch(QueuePtr)) == QUEUE_EMPTY)
			break;

		MsgPtr = &(QueuePtr->Msg[ReadPtr].MsgPtr);
		MsgLength = QueuePtr->Msg[ReadPtr].Length;
		TRACE_printf(("unet_queue_proc: MsgType=%x, MsgLength=%d", (UINT16)QueuePtr->Msg[ReadPtr].MsgType, (UINT16)MsgLength));
		Endian_Modify((*MsgPtr), QueuePtr->Msg[ReadPtr].MsgType, 0xFF);
		switch(QueuePtr->Msg[ReadPtr].MsgType){ // Issue Msg to each handler function
			case MT_ASSO_REQ:	// 0x01
				if(UnetSys.Flag.JoinNetwork && ((UnetSys.JoinRouterCnt+UnetSys.JoinEndevCnt) < UnetSys.MaxChild)) {
					UnetSys.Flag.ProcessAssociate = 1;
					unet_rx_asso_req((MCP_ASSO_REQ *)(*MsgPtr));
					UnetSys.Flag.ProcessAssociate = 0;
				}
				else {
				        DBG_puts(" ignored!");
				}
				break;
			case MT_ASSO_RSP:	// 0x02
				if(!UnetSys.Flag.JoinNetwork) {
					unet_rx_asso_rsp((MCP_ASSO_RSP *)(*MsgPtr));
				}
				else {
				        DBG_puts(" ignored!");
				}
				break;
			case MT_BEACON_REQ:	// 0x07
 				if(!UnetSys.Flag.SkipBcnReq && UnetSys.Flag.JoinNetwork && (UnetSys.DevDep < UnetSys.MaxDepth)) {
 					if(unet_tx_bcn() == SUCCESS) {
 						UnetSys.Flag.SkipBcnReq = 1;
				        	TRACE_puts(" Set SkipBcnReq");
				        }
 				}
				else {
				        DBG_puts(" ignored!");
				}
				break;
			case MT_DATA_IND:	// 0x0a
				if(UnetSys.Flag.JoinNetwork) {
					unet_rx_data_ind((UNET_HEADER **)MsgPtr, MsgLength);
					if(!UnetSys.Flag.NwkReady) {	// Network reset
						TRACE_puts("unet_queue_proc: Network reset");
						return SUCCESS;
					}
				}
				else {
				        DBG_puts(" ignored!");
				}
				break;
			case MT_BCN_IND:	// 0x0b
				if(!UnetSys.Flag.JoinNetwork) {
					unet_rx_bcn_ind((MAC_BCN_PKT *)(*MsgPtr), MsgLength);
				}
				else {
				        DBG_puts(" ignored!");
				}
				break;

			case MT_DISASSO_NTF:	// 0x03
				if(UnetSys.Flag.JoinNetwork) {
				//	Link_LED_Off();
					unet_rx_disasso((MCP_DISASSO_NTF *)(*MsgPtr));
					if(!UnetSys.Flag.NwkReady) {	// Network reset
						TRACE_puts("unet_queue_proc: Network reset");
						return SUCCESS;
					}
				}
				else {
				        DBG_puts(" ignored!");
				}
				break;
			default:
				TRACE_puts("unet_queue_proc: Unknown MsgType");
				break;
		}
	//	DisInt();
		if(QueuePtr->Msg[ReadPtr].MsgPtr)
			MSG_Free((void *)QueuePtr->Msg[ReadPtr].MsgPtr);
		QueuePtr->Msg[ReadPtr].DataFlag = false;
		QueuePtr->QueueDataCount--; // Decreaase Queue Data Count

		frequency--; // Decrease Handle frequency
	//	EnabInt();
	}

	if(UnetSys.Flag.SkipBcnReq && !QueuePtr->QueueDataCount) {
		UnetSys.Flag.SkipBcnReq = 0;
        	TRACE_puts(" Clear SkipBcnReq");
        }

//	TRACE_puts("unet_queue_proc: SUCCESS");
	return SUCCESS;

}

void unet_proc(void)
{
#if 1
	while(IntStatus.Rx) { //Received a frame
		unet_msg_dispatch();
	}
#endif


	unet_queue_proc(MSG_MAX_PROCESS, &CommQueue);

//	unet_queue_proc(MSG_MAX_QUEUE_SIZE, &DataQueue);
}

/*******************************************************************************

	Rx Msg Process Function (for ISR)

********************************************************************************/
#if 0 // DEBUG_MODE
static BOOL unet_msg_dispatch_busy = 0;
#endif // #if DEBUG_MODE

UINT8 unet_msg_dispatch(void) // REENTRANT
{
	UINT8 *Temp;
	MAC_FC FcPtr;
//	MSG_QUEUE *Qptr;
	UINT8 MsgType, RXLEN, HEADLEN;

#if 0 // DEBUG_MODE
	if(unet_msg_dispatch_busy)
		return(FAILURE);	// re-entrant from debug message output
	unet_msg_dispatch_busy = 1;
#endif // #if DEBUG_MODE

	DisInt();
	IntStatus.Rx = 0;
	EnabInt();

//	TRACE_puts("unet_msg_dispatch\x00");

	RxTx_LED_On();

	RXLEN = spi_lr(RX_N);
	HEADLEN = 3;

	if((Temp = (UINT8 *)malloc(RXLEN+2)) == NULL) { //2: LQI, RSSI
		UzRxFlush();
//		TRACE_printf(("malloc(): NULL, RXLEN=%bd", RXLEN));
		MsgType = FAILURE;
		goto exit_unet_msg_dispatch;
	}

	spi_rd_rx_fifo(Temp, RXLEN+2);

#if BIGENDIAN
	((UINT8 *)&FcPtr)[0] = Temp[1];
	((UINT8 *)&FcPtr)[1] = Temp[0];
#else
	*(UINT16 *)&FcPtr = *(UINT16 *)Temp;
#endif // #if BIGENDIAN

	LQIValue = Temp[RXLEN];
	RSSIValue = Temp[RXLEN+1];

	if(FcPtr.bits.FrmType == FRM_TYPE_BCN){
		MsgType = MT_BCN_IND;
	//	Qptr = &CommQueue;
	}else if(FcPtr.bits.FrmType == FRM_TYPE_COMM){
		if(FcPtr.bits.SrcMod == 2){
			HEADLEN += 4; // pan id + short address
		}else if(FcPtr.bits.SrcMod == 3){
			HEADLEN += 10; // pan id + long address
		}

		if(FcPtr.bits.DstMod == 2){ //16
			HEADLEN += 4; // pan id + short address
		}else if(FcPtr.bits.DstMod == 3){
			HEADLEN += 10; // pan id + long address
		}

		MsgType = *(Temp + HEADLEN);
	//	Qptr = &CommQueue;
	}
	else if(FcPtr.bits.FrmType == FRM_TYPE_DATA){
		MsgType = MT_DATA_IND;
	//	Qptr = &DataQueue;

	}else{
		free((void *)Temp);
//		TRACE_puts("unet_msg_dispatch: free(Temp)");
		MsgType = FAILURE;
		goto exit_unet_msg_dispatch;
	}

	//MSG_Copy(Temp, RF_RX_BUFF, RXLEN+2);

	if(MSG_Add((void *)Temp, MsgType , &CommQueue /* Qptr */, RXLEN) != SUCCESS){
		free((void *)Temp);
//		TRACE_puts("unet_msg_dispatch: free(Temp)");
		MsgType = FAILURE;
	}
	else
		MsgType = SUCCESS;

exit_unet_msg_dispatch:
	RxTx_LED_Off();

#if 0 // DEBUG_MODE
	unet_msg_dispatch_busy = 0;
#endif // #if DEBUG_MODE

	return(MsgType);
}

/*******************************************************************************

	Table Function

********************************************************************************/
void clear_table(void **TailAddr){
	void **Temp;

	TRACE_puts("clear_table");

	if((Temp = *TailAddr) != NULL) {
	    do {
		Temp = *Temp;
		// MSG_Copy(HeadAddr, Temp, sizeof(Temp)); // Head = Temp->Next
		MSG_Free(Temp);
	    } while(Temp != *TailAddr);
	    *TailAddr = NULL;
	}
}

void *insert_node(void **TailAddr, UINT8 TypeSize)
{
	void **Prev;
	void **New;

	TRACE_puts("insert_node: ");

//	DisInt();

	if((New = MSG_Alloc(TypeSize)) == NULL) {
		DBG_puts("NULL");
		return NULL;
	}

	if(*TailAddr) {
		Prev = *TailAddr;
		*New = *Prev;
		*Prev = New;
	}
	else
		*New = New;
	*TailAddr = New;

//	EnabInt();

	DBG_printf(("%p", New));
	return(New); // Tail = New
}

void delete_node(void **TailAddr, void **DelAddr){
	void **Prev;

	TRACE_printf(("delete_node: %p", *DelAddr));

//	DisInt();

	if((Prev = *TailAddr) == NULL) {
		DBG_puts("(empty list)");
		NOP();		// for debug break point setting
		return;		// empty list
	}

	while(*Prev != *DelAddr) {
		if(*Prev == *TailAddr) {
			TRACE_puts("(not found)");
			NOP();		// for debug break point setting
			return;		// target node not found!
		}

		Prev = *Prev;
	}
	if(*DelAddr == *TailAddr)
		*TailAddr = Prev;
	if(*TailAddr == *DelAddr)
		*TailAddr = NULL;
	else
		*Prev = *(void **)(*DelAddr);

    	MSG_Free(*DelAddr);

//	EnabInt();
}

/*******************************************************************************

	Other Function

********************************************************************************/

UINT16 power(UINT16 number, UINT8 power)
{
	UINT8 i;
	UINT16 tmp = 1;

	TRACE_printf(("power(%d, %d)", number, (UINT16)power));

	if(power == 0)
		return 1;
	else if(power == 1)
		return number;

	for(i=1;i<=power;i++){
		tmp *= number;
	}
	return tmp;
}

#ifdef I_Am_Coordinator
void clean_branch(UINT16 Caddr, UINT16 Paddr)
{
	UINT8 i, j, Depth;
	UINT16 k, Kaddr, no_of_per_depth;
	UINT16 tempdev, temprouter, tempaddr;
	UINT16 Cskip;
	struct _unet_children_table_ *ChiTabPtr;

	TRACE_printf(("clean_branch: Caddr=%04X, Paddr=%04X", Caddr, Paddr));

	if(Caddr == 0)
		return;

//	Dev = Caddr - Paddr - 1;
	tempaddr = Caddr;

	for(i=0;i<UnetSys.MaxDepth;i++){
		Cskip = compute_Cskip(i);
		tempdev = (tempaddr-1) % Cskip;
		temprouter =(tempaddr-1) / Cskip;
		if(tempdev == 0 ||temprouter>(UnetSys.MaxRouter-1)){
			Depth = i+1;
			break;
		}
		else
			tempaddr= tempdev;
	}

	for(j=1;j<=UnetSys.MaxDepth-Depth;j++){
		no_of_per_depth = power(UnetSys.MaxChild,j);
		Cskip = compute_Cskip(Depth+j-1);
		for(k=0;k<no_of_per_depth;k++){
			if(k==0)
				Kaddr = Caddr + 1;
			else if(k%UnetSys.MaxChild != 0)
				Kaddr+= Cskip;
			else
				Kaddr+= Cskip + 1;

			if((ChiTabPtr = UnetSys.ChildTableTail) != NULL) {
			    do {
				ChiTabPtr = ChiTabPtr->Next;
				if(ChiTabPtr->NwkAddress == Kaddr){
					if(UnetSys.Flag.Kick)//set permit join OFF
						TxUnetLocalQuery(Kaddr, 0x88000088, 0);

					TRACE_printf(("clean_branch: Kaddr=%04X", Kaddr));
					delete_node((void *)&UnetSys.ChildTableTail, (void *)&ChiTabPtr);
					break;
				}
			    } while(ChiTabPtr != UnetSys.ChildTableTail);
			}
		}
		Caddr+=1;
	}
}
#endif

UINT16 compute_Cskip(INT8 depth)
{
	INT16 RmT, Tmp;
	UINT8 i;

	TRACE_printf(("compute_Cskip(%d)=", (UINT16)depth));

#if 0
	DBG_printf((" (MaxChild=%d, MaxRouter=%d, MaxDepth=%d) ", (UINT16)UnetSys.MaxRouter, (UINT16)UnetSys.MaxChild, (UINT16)UnetSys.MaxDepth));
#endif

	if(UnetSys.MaxRouter ==1) {
		Tmp = 1 + (INT16)UnetSys.MaxChild * ((INT16)UnetSys.MaxDepth - depth - 1);
	}
	else {
		if((Tmp = ((INT16)UnetSys.MaxDepth - depth -1)) == 0){
			RmT = 1;
		}else if(Tmp < 0){
			RmT = (1 / UnetSys.MaxChild);
		}else{
			RmT = 1;
			for(i = 0; i < Tmp ; i++){
				RmT *= (INT16)UnetSys.MaxRouter;
			}
		}

		Tmp = ((INT16)(1 + UnetSys.MaxChild - UnetSys.MaxRouter) - (INT16)UnetSys.MaxChild * RmT) / (1 - (INT16)UnetSys.MaxRouter);
	}

	DBG_printf(("%d", Tmp));
	return (Tmp);
}
